// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DriveFeedbackStateTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.DAC
{
    using System;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The Drive state feedback unit test class
    /// </summary>
    [TestClass]
    public class DriveStateFeedbackTest
    {
        /// <summary>
        /// The wheel base meters.
        /// </summary>
        private const double WheelBaseMeters = 1.0;

        /// <summary>
        /// The wheel position delta meters.
        /// </summary>
        private const double WheelPositionDeltaMeters = 1.0;

        /// <summary>
        /// The default resolution.
        /// </summary>
        private const double DefaultResolution = 1.0;

        /// <summary>
        /// The num tests for average results.
        /// </summary>
        private const int NumTestsForAverageResults = 100;

        /// <summary>
        /// The required within po m one sigma.
        /// </summary>
        private const int RequiredWithinPoMOneSigma = 60;

        /// <summary>
        /// The previous drive feedback state used as reference.
        /// </summary>
        private DriveFeedbackState previousDriveFeedbackState = new DriveFeedbackState(0, 0, WheelBaseMeters, new OdometryNoiseModelParams(), null);

        /// <summary>
        /// The previous drive feedback state with heading used as reference.
        /// </summary>
        private DriveFeedbackState previousDriveFeedbackStateWithHeading = new DriveFeedbackState(0, 0, 0, WheelBaseMeters, new OdometryNoiseModelParams(), null);

        /// <summary>
        /// Default noisy odometry
        /// </summary>
        private OdometryNoiseModelParams noisyOdometry = new OdometryNoiseModelParams() { HeadingStdDevInRadians = 0.001, LeftWheelStdDevInMeters = 0.001, RightWheelStdDevInMeters = 0.001, IgnoreNoiseParams = false };

        /// <summary>
        /// Noisy heading only, no wheel noise.
        /// </summary>
        private OdometryNoiseModelParams noisyHeading = new OdometryNoiseModelParams() { HeadingStdDevInRadians = 0.001, LeftWheelStdDevInMeters = 0.0, RightWheelStdDevInMeters = 0.0, IgnoreNoiseParams = false };

        /// <summary>
        /// Noisy wheel only, no heading noise.
        /// </summary>
        private OdometryNoiseModelParams noisyWheels = new OdometryNoiseModelParams() { HeadingStdDevInRadians = 0.0, LeftWheelStdDevInMeters = 0.001, RightWheelStdDevInMeters = 0.001, IgnoreNoiseParams = false };

        /// <summary>
        /// Heading noisier than wheel. We should trust wheels more.
        /// </summary>
        private OdometryNoiseModelParams headingNoisierThanWheels = new OdometryNoiseModelParams() { HeadingStdDevInRadians = 0.01, LeftWheelStdDevInMeters = 0.001, RightWheelStdDevInMeters = 0.001, IgnoreNoiseParams = false };

        /// <summary>
        /// Wheels noisier than heading. We should trust heading more.
        /// </summary>
        private OdometryNoiseModelParams wheelsNoisierThanHeading = new OdometryNoiseModelParams() { HeadingStdDevInRadians = 0.001, LeftWheelStdDevInMeters = 0.01, RightWheelStdDevInMeters = 0.01, IgnoreNoiseParams = false };

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateRotateClean()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(-WheelBaseMeters * 0.5 * MathConstants.PIOverFour, WheelBaseMeters * 0.5 * MathConstants.PIOverFour, WheelBaseMeters, new OdometryNoiseModelParams(), this.previousDriveFeedbackState);        

            Pose2D pose = ddmm.GetUpdatedPose(new Pose2D(0, 0, 0), this.previousDriveFeedbackState);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == MathConstants.PIOverFour);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateRotateCleanWithPreviousHeading()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(-WheelBaseMeters * 0.5 * MathConstants.PIOverFour, WheelBaseMeters * 0.5 * MathConstants.PIOverFour, WheelBaseMeters, new OdometryNoiseModelParams(), this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.GetUpdatedPose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == MathConstants.PIOverFour);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateRotateCleanVariedSample()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(-WheelBaseMeters * 0.5 * MathConstants.PIOverFour, WheelBaseMeters * 0.5 * MathConstants.PIOverFour, WheelBaseMeters, new OdometryNoiseModelParams(), this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.GetUpdatedPose(new Pose2D(1.0, 1.0, MathConstants.PIOverFour), this.previousDriveFeedbackStateWithHeading);
            Assert.AreEqual(pose.X, 1.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Y, 1.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Heading, MathConstants.PIOverTwo, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateNoisyHeadingNoMotionNoRotationShouldResultInNoNoise()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, WheelBaseMeters, this.noisyHeading, this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.SamplePose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == 0.0);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateNoisyWheelsNoMotionNoRotationShouldResultInNoNoise()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, WheelBaseMeters, this.noisyWheels, this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.SamplePose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == 0.0);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateNoisyOdoNoMotionNoRotationShouldResultInNoNoise()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, WheelBaseMeters, this.noisyOdometry, this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.SamplePose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.IsTrue(pose.X == 0.0 && pose.Y == 0.0 && pose.Heading == 0.0);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateNoisyHeadingOnlyNoMotionWithRotationShouldResultInSmallNoise()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, Math.PI, WheelBaseMeters, this.noisyHeading, this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.SamplePose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.AreEqual(0.0, pose.X, this.noisyWheels.LeftWheelStdDevInMeters * 100);
            Assert.AreEqual(0.0, pose.Y, this.noisyWheels.RightWheelStdDevInMeters * 100);
            Assert.AreEqual(0.0, pose.Heading, this.noisyWheels.HeadingStdDevInRadians * 100);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateNoisyWheelsOnlyNoMotionWithRotationShouldResultInSmallNoise()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, Math.PI, WheelBaseMeters, this.noisyWheels, this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.SamplePose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.AreEqual(0.0, pose.X, this.noisyWheels.LeftWheelStdDevInMeters * 100);
            Assert.AreEqual(0.0, pose.Y, this.noisyWheels.RightWheelStdDevInMeters * 100);
            Assert.AreEqual(Math.PI, pose.Heading, this.noisyWheels.HeadingStdDevInRadians * 100);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateNoisyOdometryNoMotionWithRotationShouldResultInNoise()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, Math.PI, WheelBaseMeters, this.noisyOdometry, this.previousDriveFeedbackStateWithHeading);

            Pose2D pose = ddmm.SamplePose(new Pose2D(0, 0, 0), this.previousDriveFeedbackStateWithHeading);
            Assert.AreEqual(pose.X, 0.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Y, 0.0, MathConstants.ErrorEpsilon);
            Assert.AreEqual(pose.Heading, 1.83879, 0.01);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStatePoseTestGetPoseUpdate()
        {
            IDriveState previousDddm = new PoseFeedbackState(new Pose2D(), new PoseNoiseModelParams());
            Pose2D poseUpdate = new Pose2D(1, 0.5, MathConstants.PIOverFour);
            PoseFeedbackState dddm = new PoseFeedbackState(poseUpdate, new PoseNoiseModelParams());
            Pose2D pose = dddm.GetUpdatedPose(new Pose2D(0, 0, 0), previousDddm);
            Assert.AreEqual(poseUpdate.X, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseUpdate.Y, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseUpdate.Heading, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStatePoseTestGetMeanDeltaPoseUpdate()
        {
            IDriveState previousDddm = new PoseFeedbackState(new Pose2D(), new PoseNoiseModelParams());
            Pose2D poseUpdate = new Pose2D(1, 0.5, MathConstants.PIOverFour);
            PoseFeedbackState dddm = new PoseFeedbackState(poseUpdate, new PoseNoiseModelParams());
            Pose2D pose = dddm.GetMeanDeltaPose(previousDddm);
            Assert.AreEqual(poseUpdate.X, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseUpdate.Y, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseUpdate.Heading, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test getting the pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStatePoseTestGetMeanDeltaPoseUpdateWithFitness()
        {
            IDriveState previousDddm = new PoseFeedbackState(new Pose2D(), new PoseNoiseModelParams());
            Pose2D poseUpdate = new Pose2D(1, 0.5, MathConstants.PIOverFour);
            PoseFeedbackState dddm = new PoseFeedbackState(poseUpdate, new PoseNoiseModelParams());
            double fitness;
            Pose2D pose = dddm.GetMeanDeltaPose(previousDddm, out fitness);
            Assert.AreEqual(poseUpdate.X, pose.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseUpdate.Y, pose.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseUpdate.Heading, pose.Heading, MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Ensure that drive feedback state does not maintain link to all the previous nominal states.
        /// This was causing issues with serialization.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void DriveFeedbackStateSerializationTest()
        {
            DriveFeedbackState ddmm = new DriveFeedbackState(0, 0, Math.PI, WheelBaseMeters, this.noisyHeading, this.previousDriveFeedbackStateWithHeading);
            DriveFeedbackState ddmmNext = new DriveFeedbackState(0, 0, Math.PI, WheelBaseMeters, this.noisyHeading, ddmm);
            Assert.IsNull(ddmmNext.PreviousNominalState.PreviousNominalState);
        }
    }
}
